package com.neuronrobotics.aim.activecannula;

import java.awt.event.ActionEvent;
import java.awt.event.ActionListener;

import javax.swing.AbstractButton;
import javax.swing.JButton;
import javax.swing.JLabel;
import javax.swing.JPanel;
import javax.swing.JTextField;

import com.neuronrobotics.aim.gui.MainGUI;
import com.neuronrobotics.sdk.genericdevice.GenericPIDDevice;
import com.neuronrobotics.sdk.pid.IPIDEventListener;
import com.neuronrobotics.sdk.pid.PIDConfiguration;
import com.neuronrobotics.sdk.pid.PIDEvent;
import com.neuronrobotics.sdk.pid.PIDLimitEvent;

import net.miginfocom.swing.MigLayout;
import java.awt.Color;
import java.awt.Font;

public class ActiveCannulaControlPanel0134 extends JPanel implements
		IPIDEventListener {
	private static final long serialVersionUID = 1L;
	
	protected static final AbstractButton textFieldResetEncoderInnerTranlsation = null;
	private final JLabel lblOuterTubeTranslation = new JLabel(
			"Outer Tube Translation");
	private final JLabel lblDesiredJointPos = new JLabel("Desired Joint Pos");
	private final JLabel lblActualJointPos = new JLabel("Actual Joint Pos");
	private final JLabel lblTime = new JLabel("Time");
	private final JLabel lblMiddleTubeTranslation = new JLabel(
			"Middle Tube Translation");
	private final JTextField textFieldDesiredOuterTranslation = new JTextField();
	private final JTextField textFieldActualOuterTranslation = new JTextField();
	private final JTextField textFieldTimeOuterTranslation = new JTextField();
	private final JTextField textFieldDesiredMiddleTranslation = new JTextField();
	private final JTextField textFieldActualMiddleTranslation = new JTextField();
	private final JTextField textFieldTimeMiddle = new JTextField();
	private final JLabel lblMiddleTubeRotation = new JLabel(
			"Middle Tube Rotation");
	private final JLabel lblInnerTubeTranslation = new JLabel(
			"Inner Tube Translation");
	private final JTextField textFieldDesiredMiddleRotation = new JTextField();
	private final JTextField textFieldDesiredInnerTranslation = new JTextField();
	private final JTextField textFieldActualMiddleRotation = new JTextField();
	private final JTextField textFieldActualInnerTranslation = new JTextField();
	private final JTextField textFieldTimeInnerTranslation = new JTextField();
	private final JButton btnStop = new JButton("STOP");
	private final JButton btnMoveOuterTranslation = new JButton("Move");
	private final JButton btnMoveMiddle = new JButton("Move");
	private final JButton btnMoveInnerTranslation = new JButton("Move");
	private final JButton btnHome = new JButton("Home");
	private final JButton buttonJogPlusOuterTranslation = new JButton("Jog+");
	private final JButton buttonJogMinusOuterTranslation = new JButton("Jog-");
	private final JButton buttonJogPlusMiddleTranslation = new JButton("Jog+");
	private final JButton buttonJogPlusMiddleRotation = new JButton("Jog+");
	private final JButton buttonJogPlusInnerTranslation = new JButton("Jog+");
	private final JButton buttonJogMinusMiddleTranslation = new JButton("Jog-");
	private final JButton buttonJogMinusMiddleRotation = new JButton("Jog-");
	private final JButton buttonJogMinusInnerTranslation = new JButton("Jog-");
	

	// Linear encoder convention ratio: 1 inch has 500 ticks==0.02032
	private final double encoderTick2mm= 0.0508;
	// Rotary encoder convention ratio: 1 round has 1250 ticks=360/1250=0.288
	private final double mm2EncoderTick =19.6850394;
	
	private final double encoderTicks2degree= 0.288;
	private final double degree2EncoderTick= 3.47222222;
	
	
	private int[] currentJointTicksValues = new int[5];
	private double[] currentJointSIValues = new double[5];
	
	private final JTextField textFieldJogStep = new JTextField();
	private final JLabel labelJogStepUnits = new JLabel("mm/deg");
	
	private int[] desiredJointTicksInteger = new int[5];

	
	public int calibratedOutTranslate=0;
	public int calibratedMiddleTranslate=0;
	public int calibratedMiddleRotate=0;
	public int calibratedInnerTranslate=0;

	private MainGUI kinematicsGUI;
	private final JLabel lblAxis = new JLabel("Axis 0");
	private final JLabel lblAxis_1 = new JLabel("Axis 1");
	private final JLabel lblAxis_2 = new JLabel("Axis 3");
	private final JLabel lblAxis_3 = new JLabel("Axis 4");
	private final JButton btnResetEncoder = new JButton("Reset");
	private final JButton btnPauseOuter = new JButton("Pause");
	private final JButton btnPauseMiddleTranslation = new JButton("Pause");
	private final JButton btnPauseInnerTranslation = new JButton("Pause");
	private final JButton btnStart = new JButton("Start");
	
	public ActiveCannulaControlPanel0134(MainGUI kinematicsGUI) {
		setKinematicsGUI(kinematicsGUI);
		
		setLayout(new MigLayout("", "[22px][][39px][::60px,grow][:30.00:60px][][94.00][][::85.00px,grow][][][][][]", "[20px][][][][][][][][][][][:56.00:30px][][]"));
		btnStart.addActionListener(new ActionListener() {
			public void actionPerformed(ActionEvent e) {
				initialize();				
			}
		});
		
		add(btnStart, "cell 2 0");
		textFieldJogStep.setText("1");
		textFieldJogStep.setColumns(10);
		
		add(textFieldJogStep, "cell 4 1,alignx center");		
		add(labelJogStepUnits, "cell 5 1,alignx center");
		add(lblDesiredJointPos, "cell 6 1,alignx center");
		add(lblActualJointPos, "cell 8 1,alignx center");
		add(lblTime, "cell 10 1,alignx center");
		add(lblAxis, "cell 1 2");
		
		lblOuterTubeTranslation.setFont(new Font("Tahoma", Font.PLAIN, 15));
		lblOuterTubeTranslation.setForeground(Color.BLUE);
		add(lblOuterTubeTranslation, "cell 2 2,alignx trailing");
		
		btnMoveOuterTranslation.addActionListener(new ActionListener() {
			public void actionPerformed(ActionEvent e) {	
				double seconds = Double.parseDouble(textFieldTimeOuterTranslation.getText());
				double desiredOuterTranslationSI = Double.parseDouble(textFieldDesiredOuterTranslation.getText());				
				int desiredOuterTranslationTicks = (int)(desiredOuterTranslationSI*encoderTick2mm);	
				
				setDesiredJointTickValue(0,desiredOuterTranslationTicks);		
				runDesiredJointChannelUpdate(0, seconds);				
			}
		});
		buttonJogPlusOuterTranslation.addActionListener(new ActionListener() {
			boolean isInitialized= false;
			public void actionPerformed(ActionEvent e) {
				isInitialized = true;
				double jogStep = Double.parseDouble(textFieldJogStep.getText());				
				double encoderOuterTranslation =getCurrentJointTicksValues()[0]*encoderTick2mm;				
								
				if (isInitialized==true){
					encoderOuterTranslation =Double.parseDouble(textFieldDesiredOuterTranslation.getText()); 
				}
									
				double encoderOuterTranslationJog =encoderOuterTranslation+jogStep;	
				textFieldDesiredOuterTranslation.setText(Double.toString(encoderOuterTranslationJog));
				int encoderOuterTranslationJogStepTick =(int) (encoderOuterTranslationJog*mm2EncoderTick);
				
				setDesiredJointTickValue(0,encoderOuterTranslationJogStepTick);						
				runDesiredJointChannelUpdate(0, 0.5);
				
			}
		});

		add(buttonJogPlusOuterTranslation, "cell 4 2");
		buttonJogMinusOuterTranslation.addActionListener(new ActionListener() {
			boolean isInitialized= false;
			public void actionPerformed(ActionEvent e) {
				isInitialized = true;
				double jogStep = Double.parseDouble(textFieldJogStep.getText());
				double encoderOuterTranslation =getCurrentJointTicksValues()[0]*encoderTick2mm;				
				
				if (isInitialized==true){
					encoderOuterTranslation =Double.parseDouble(textFieldDesiredOuterTranslation.getText()); 
				}
				
				double encoderOuterTranslationJog =encoderOuterTranslation-jogStep;	
				textFieldDesiredOuterTranslation.setText(Double.toString(encoderOuterTranslationJog));
				int encoderOuterTranslationJogStepTick =(int) (encoderOuterTranslationJog*mm2EncoderTick);
				
				setDesiredJointTickValue(0,encoderOuterTranslationJogStepTick);						
				runDesiredJointChannelUpdate(0, 0.5);
				
			}
		});

		add(buttonJogMinusOuterTranslation, "cell 5 2");
		textFieldDesiredOuterTranslation.setText("0");
		textFieldDesiredOuterTranslation.setColumns(10);
		add(textFieldDesiredOuterTranslation, "cell 6 2,alignx left");

		textFieldDesiredOuterTranslation
				.addActionListener(new ActionListener() {
					public void actionPerformed(ActionEvent e) {
					}
				});
		textFieldActualOuterTranslation.setEditable(false);
		textFieldActualOuterTranslation.setText("0");
		textFieldActualOuterTranslation.setColumns(10);

		add(textFieldActualOuterTranslation, "cell 8 2");
		textFieldTimeOuterTranslation.setText("3");
		textFieldTimeOuterTranslation.setColumns(10);
		add(textFieldTimeOuterTranslation, "cell 10 2");
		btnPauseOuter.addActionListener(new ActionListener() {
			public void actionPerformed(ActionEvent e) {
				double actualOuterTranslation= Double.parseDouble(textFieldActualOuterTranslation.getText());				
				textFieldDesiredOuterTranslation.setText(textFieldActualOuterTranslation.getText());	
				setDesiredJointTickValue(0, (int)(actualOuterTranslation*mm2EncoderTick));
				double seconds = Double.parseDouble(textFieldTimeOuterTranslation.getText()); 
				runDesiredJointChannelUpdate(0, seconds);				
			}
		});
		
		add(btnPauseOuter, "cell 11 2");
		add(btnMoveOuterTranslation, "cell 13 2");
		
		add(lblAxis_1, "cell 1 4");
		lblMiddleTubeTranslation.setFont(new Font("Tahoma", Font.PLAIN, 15));
		lblMiddleTubeTranslation.setForeground(Color.BLUE);
		add(lblMiddleTubeTranslation, "cell 2 4,alignx trailing");

		textFieldDesiredMiddleTranslation
				.addActionListener(new ActionListener() {
					public void actionPerformed(ActionEvent e) {
					}
				});
		buttonJogPlusMiddleTranslation.addActionListener(new ActionListener() {
			boolean isInitialized = false;
			public void actionPerformed(ActionEvent e) {
				isInitialized = true;
				double jogStep = Double.parseDouble(textFieldJogStep.getText());
				double encoderMiddleTranslation =getCurrentJointTicksValues()[1]*encoderTick2mm;	
				
				if (isInitialized==true){
					encoderMiddleTranslation =Double.parseDouble(textFieldDesiredMiddleTranslation.getText()); 
				}
				
				double encoderMiddleTranslationJog =encoderMiddleTranslation+jogStep;		
				textFieldDesiredMiddleTranslation.setText(Double.toString(encoderMiddleTranslationJog));
				int encoderMiddleTranslationJogStepTick =(int) (encoderMiddleTranslationJog*mm2EncoderTick);
				
				setDesiredJointTickValue(1,encoderMiddleTranslationJogStepTick);						
				runDesiredJointChannelUpdate(0, 0.5);
				
			}
		});

		add(buttonJogPlusMiddleTranslation, "cell 4 4");
		buttonJogMinusMiddleTranslation.addActionListener(new ActionListener() {
			boolean isInitialized = false;
			public void actionPerformed(ActionEvent e) {

				isInitialized = true;
				double jogStep = Double.parseDouble(textFieldJogStep.getText());
				double encoderMiddleTranslation =getCurrentJointTicksValues()[1]*encoderTick2mm;	
				
				if (isInitialized==true){
					encoderMiddleTranslation =Double.parseDouble(textFieldDesiredMiddleTranslation.getText()); 
				}
				
				double encoderMiddleTranslationJog =encoderMiddleTranslation-jogStep;		
				textFieldDesiredMiddleTranslation.setText(Double.toString(encoderMiddleTranslationJog));
				int encoderMiddleTranslationJogStepTick =(int) (encoderMiddleTranslationJog*mm2EncoderTick);
				
				setDesiredJointTickValue(1,encoderMiddleTranslationJogStepTick);						
				runDesiredJointChannelUpdate(0, 0.5);
								
			}
		});

		add(buttonJogMinusMiddleTranslation, "cell 5 4");
		textFieldDesiredMiddleTranslation.setText("0");
		textFieldDesiredMiddleTranslation.setColumns(10);
		add(textFieldDesiredMiddleTranslation, "cell 6 4,growx");
		textFieldActualMiddleTranslation.setEditable(false);
		textFieldActualMiddleTranslation.setText("0");
		textFieldActualMiddleTranslation.setColumns(10);
		add(textFieldActualMiddleTranslation, "cell 8 4,growx");
		textFieldTimeMiddle.setText("3");
		textFieldTimeMiddle.setColumns(10);
		add(textFieldTimeMiddle, "cell 10 4,growx");
		btnPauseMiddleTranslation.addActionListener(new ActionListener() {
			public void actionPerformed(ActionEvent e) {
				double actualMiddleTranslation= Double.parseDouble(textFieldActualMiddleTranslation.getText());				
				textFieldDesiredOuterTranslation.setText(textFieldActualMiddleTranslation.getText());	
				setDesiredJointTickValue(1, (int)(actualMiddleTranslation*mm2EncoderTick));
				
				double actualMiddleRotation= Double.parseDouble(textFieldActualMiddleTranslation.getText());				
				textFieldDesiredMiddleTranslation.setText(textFieldActualMiddleRotation.getText());	
				setDesiredJointTickValue(3, (int)(actualMiddleRotation*degree2EncoderTick));
				
				double seconds = Double.parseDouble(textFieldTimeMiddle.getText()); 
				runDesiredJointChannelUpdate(1, seconds);
				runDesiredJointChannelUpdate(3, seconds);
				
				
			}
		});
		
		add(btnPauseMiddleTranslation, "cell 11 4");
		btnMoveMiddle.addActionListener(new ActionListener() {
			public void actionPerformed(ActionEvent e) {
				
				double seconds = Double.parseDouble(textFieldTimeMiddle.getText());
				double desiredMiddleTranslationSI = Double.parseDouble(textFieldDesiredMiddleTranslation.getText());
				int desiredMiddleTranslationTicks = (int)(desiredMiddleTranslationSI*encoderTick2mm);	
				setDesiredJointTickValue(1,desiredMiddleTranslationTicks);	
				
				double desiredMiddleRotationSI = Double.parseDouble(textFieldDesiredMiddleRotation.getText());
				int desiredMiddleRotationTicks = (int)(desiredMiddleRotationSI*encoderTicks2degree);	
				setDesiredJointTickValue(3,desiredMiddleRotationTicks);	
								
				runDesiredJointChannelUpdate(1, seconds);
				runDesiredJointChannelUpdate(3, seconds);
				
			}
		});
		add(btnMoveMiddle, "cell 13 4");
		
		add(lblAxis_2, "cell 1 6");
		lblMiddleTubeRotation.setForeground(Color.BLUE);
		lblMiddleTubeRotation.setFont(new Font("Tahoma", Font.PLAIN, 15));
		add(lblMiddleTubeRotation, "cell 2 6,alignx center");

		textFieldDesiredMiddleRotation.addActionListener(new ActionListener() {
			public void actionPerformed(ActionEvent e) {
			}
		});
		buttonJogPlusMiddleRotation.addActionListener(new ActionListener() {
			boolean isInitialized = false;
			public void actionPerformed(ActionEvent e) {
				
				isInitialized = true;
				double jogStep = Double.parseDouble(textFieldJogStep.getText());
				double encoderMiddleRotation =getCurrentJointTicksValues()[1]*encoderTick2mm;	
				
				if (isInitialized==true){
					encoderMiddleRotation =Double.parseDouble(textFieldDesiredMiddleRotation.getText()); 
				}
				
				double encoderMiddleRotationJog =encoderMiddleRotation+jogStep;		
				textFieldDesiredMiddleRotation.setText(Double.toString(encoderMiddleRotationJog));
				int encoderMiddleRotationJogStepTick =(int) (encoderMiddleRotationJog*degree2EncoderTick);
				
				setDesiredJointTickValue(3,encoderMiddleRotationJogStepTick);						
				runDesiredJointChannelUpdate(3, 0.5);
				
			}
		});

		add(buttonJogPlusMiddleRotation, "cell 4 6");
		buttonJogMinusMiddleRotation.addActionListener(new ActionListener() {
			boolean isInitialized = false;
			public void actionPerformed(ActionEvent e) {
				
				isInitialized = true;
				double jogStep = Double.parseDouble(textFieldJogStep.getText());
				double encoderMiddleRotation =getCurrentJointTicksValues()[1]*encoderTick2mm;	
				
				if (isInitialized==true){
					encoderMiddleRotation =Double.parseDouble(textFieldDesiredMiddleRotation.getText()); 
				}
				
				double encoderMiddleRotationJog =encoderMiddleRotation-jogStep;		
				textFieldDesiredMiddleRotation.setText(Double.toString(encoderMiddleRotationJog));
				int encoderMiddleRotationJogStepTick =(int) (encoderMiddleRotationJog*degree2EncoderTick);
				
				setDesiredJointTickValue(3,encoderMiddleRotationJogStepTick);						
				runDesiredJointChannelUpdate(3, 0.5);
				
				
			}
		});

		add(buttonJogMinusMiddleRotation, "cell 5 6");
		textFieldDesiredMiddleRotation.setText("0");
		textFieldDesiredMiddleRotation.setColumns(10);
		add(textFieldDesiredMiddleRotation, "cell 6 6,growx");
		textFieldActualMiddleRotation.setEditable(false);
		textFieldActualMiddleRotation.setText("0");
		textFieldActualMiddleRotation.setColumns(10);
		add(textFieldActualMiddleRotation, "cell 8 6,growx");
		
		add(lblAxis_3, "cell 1 8");
		lblInnerTubeTranslation.setForeground(Color.BLUE);
		lblInnerTubeTranslation.setFont(new Font("Tahoma", Font.PLAIN, 15));
		add(lblInnerTubeTranslation, "cell 2 8,alignx center");

		btnMoveInnerTranslation.addActionListener(new ActionListener() {
			public void actionPerformed(ActionEvent e) {
				
				double seconds = Double.parseDouble(textFieldTimeInnerTranslation.getText());
				double desiredInnerTranslationSI = Double.parseDouble(textFieldDesiredOuterTranslation.getText());				
				int desiredInnerTranslationTicks = (int)(desiredInnerTranslationSI*encoderTick2mm);	
				
				setDesiredJointTickValue(4,desiredInnerTranslationTicks);		
				runDesiredJointChannelUpdate(4, seconds);
				
			}
		});

		textFieldDesiredInnerTranslation.addActionListener(new ActionListener() {
			public void actionPerformed(ActionEvent e) {
			}
		});
		buttonJogPlusInnerTranslation.addActionListener(new ActionListener() {
			boolean isInitialized = false;
			public void actionPerformed(ActionEvent e) {
				
				isInitialized = true;
				double jogStep = Double.parseDouble(textFieldJogStep.getText());
				double encoderInnerTranslation =getCurrentJointTicksValues()[1]*encoderTick2mm;	
				
				if (isInitialized==true){
					encoderInnerTranslation =Double.parseDouble(textFieldDesiredInnerTranslation.getText()); 
				}
				
				double encoderInnerTranslationJog =encoderInnerTranslation+jogStep;		
				textFieldDesiredInnerTranslation.setText(Double.toString(encoderInnerTranslationJog));
				int encoderInnerTranslationJogStepTick =(int) (encoderInnerTranslationJog*mm2EncoderTick);
				
				setDesiredJointTickValue(4,encoderInnerTranslationJogStepTick);						
				runDesiredJointChannelUpdate(4, 0.5);
				
				
			}
		});

		add(buttonJogPlusInnerTranslation, "cell 4 8");
		buttonJogMinusInnerTranslation.addActionListener(new ActionListener() {
			boolean isInitialized = false;
			public void actionPerformed(ActionEvent e) {
				
				isInitialized = true;
				double jogStep = Double.parseDouble(textFieldJogStep.getText());
				double encoderInnerTranslation =getCurrentJointTicksValues()[1]*encoderTick2mm;	
				
				if (isInitialized==true){
					encoderInnerTranslation =Double.parseDouble(textFieldDesiredInnerTranslation.getText()); 
				}
				
				double encoderInnerTranslationJog =encoderInnerTranslation-jogStep;		
				textFieldDesiredInnerTranslation.setText(Double.toString(encoderInnerTranslationJog));
				int encoderInnerTranslationJogStepTick =(int) (encoderInnerTranslationJog*mm2EncoderTick);
				
				setDesiredJointTickValue(4,encoderInnerTranslationJogStepTick);						
				runDesiredJointChannelUpdate(4, 0.5);
				
				
			}
		});

		add(buttonJogMinusInnerTranslation, "cell 5 8");
		textFieldDesiredInnerTranslation.setText("0");
		textFieldDesiredInnerTranslation.setColumns(10);
		add(textFieldDesiredInnerTranslation, "cell 6 8,growx");
		textFieldActualInnerTranslation.setEditable(false);
		textFieldActualInnerTranslation.setText("0");
		textFieldActualInnerTranslation.setColumns(10);
		add(textFieldActualInnerTranslation, "cell 8 8,growx");
		textFieldTimeInnerTranslation.setText("3");
		textFieldTimeInnerTranslation.setColumns(10);
		add(textFieldTimeInnerTranslation, "cell 10 8,growx");
		btnPauseInnerTranslation.addActionListener(new ActionListener() {
			public void actionPerformed(ActionEvent e) {
				
				double actualInnerTranslation= Double.parseDouble(textFieldActualInnerTranslation.getText());				
				textFieldDesiredInnerTranslation.setText(textFieldActualInnerTranslation.getText());	
				setDesiredJointTickValue(4, (int)(actualInnerTranslation*mm2EncoderTick));
				double seconds = Double.parseDouble(textFieldTimeInnerTranslation.getText()); 
				runDesiredJointChannelUpdate(4, seconds);
							
			}
		});
		
		add(btnPauseInnerTranslation, "cell 11 8");

		add(btnMoveInnerTranslation, "cell 13 8");
				btnStop.setForeground(Color.RED);
				btnStop.setFont(new Font("Tahoma", Font.BOLD, 15));
				btnStop.setBounds(91, 332, 80, 55);
				
						btnStop.addActionListener(new ActionListener() {
							public void actionPerformed(ActionEvent e) {								
								stopPID();
								
							}
						});
						btnResetEncoder.addActionListener(new ActionListener() {
							public void actionPerformed(ActionEvent e) {
								stopPID();
								
								calibratedOutTranslate= (int)(Double.parseDouble(textFieldActualOuterTranslation.getText())*mm2EncoderTick);
								calibratedMiddleTranslate= (int)(Double.parseDouble(textFieldActualMiddleTranslation.getText())*mm2EncoderTick);
								calibratedMiddleRotate= (int)(Double.parseDouble(textFieldActualMiddleRotation.getText())*degree2EncoderTick);
								calibratedInnerTranslate= (int)(Double.parseDouble(textFieldActualInnerTranslation.getText())*mm2EncoderTick);
								
								setDesiredJointTickValue(0, calibratedOutTranslate);
								setDesiredJointTickValue(1, calibratedMiddleTranslate);
								setDesiredJointTickValue(3, calibratedMiddleRotate);
								setDesiredJointTickValue(4, calibratedInnerTranslate);
								
								
								textFieldDesiredOuterTranslation.setText(textFieldActualOuterTranslation.getText());
								textFieldDesiredMiddleTranslation.setText(textFieldActualMiddleTranslation.getText());
								textFieldDesiredMiddleRotation.setText(textFieldActualMiddleRotation.getText());
								textFieldDesiredInnerTranslation.setText(textFieldActualInnerTranslation.getText());
												
								resumePID();
								
							}
						});
						
						add(btnResetEncoder, "cell 3 11");
						btnHome.addActionListener(new ActionListener() {
							public void actionPerformed(ActionEvent e) {

								setDesiredJointTickValue(0, 0);
								setDesiredJointTickValue(1, 0);
								setDesiredJointTickValue(3, 0);
								setDesiredJointTickValue(4, 0);
								
								runDesiredJointUpdate(3);
							}
						});
						btnHome.setFont(new Font("Tahoma", Font.PLAIN, 15));
						add(btnHome, "cell 6 11,alignx center");
						add(btnStop, "cell 8 11,alignx center");

	}

	
	private void initialize() {
		
		
		kinematicsGUI.getDevice().addPIDEventListener(this);
		
		double temp= kinematicsGUI.getDevice().GetPIDPosition(0)*encoderTick2mm;
		textFieldActualOuterTranslation.setText(Double.toString(temp));
		
		temp= kinematicsGUI.getDevice().GetPIDPosition(1)*encoderTick2mm;
		textFieldActualMiddleTranslation.setText(Double.toString(temp));
		
		
		temp= kinematicsGUI.getDevice().GetPIDPosition(3)*encoderTick2mm;
		textFieldActualMiddleRotation.setText(Double.toString(temp));
		
		temp= kinematicsGUI.getDevice().GetPIDPosition(4)*encoderTick2mm;
		textFieldActualInnerTranslation.setText(Double.toString(temp));
	}


	@Override
	public void onPIDEvent(PIDEvent e) {
		System.err
		.println("Got a PID event in Kinematics GUI #" + e.getGroup()
				+ " value: " + e.getValue() + " timestamp: "
				+ e.getTimeStamp());
		// getCurrentJointTicksValues() is 5x1 vector
		getCurrentJointTicksValues()[e.getGroup()] = e.getValue();
			
//		double outerTranslation = (getCurrentJointTicksValues()[0]-calibratedOutTranslate)*encoderTick2mm;
//		System.out.println(outerTranslation);
//		System.out.println(encoderTick2mm);
//		textFieldActualOuterTranslation.setText(Double.toString(outerTranslation));
		//textFieldActualOuterTranslation.setText(Double.toString(getCurrentJointTicksValues()[0]));
		
		updateRobotJointDisplay(getCurrentJointTicksValues());
	}

	@Override
	public void onPIDLimitEvent(PIDLimitEvent e) {
		
	}

	@Override
	public void onPIDReset(int group, int currentValue) {

	}
	
	public void updateRobotJointDisplay (int[] currentJointTicks){
		
		double outerTranslation = (getCurrentJointTicksValues()[0]-calibratedOutTranslate)*encoderTick2mm;
		textFieldActualOuterTranslation.setText(Double.toString(outerTranslation));
		
		double middleTranslation =(getCurrentJointTicksValues()[1]-calibratedMiddleTranslate)*encoderTick2mm;
		textFieldActualMiddleTranslation.setText(Double.toString(middleTranslation));
		
		double middleRotation = (getCurrentJointTicksValues()[3]-calibratedMiddleRotate)*encoderTicks2degree;
		textFieldActualMiddleRotation.setText(Double.toString(middleRotation));
		
		double innerTranslation = (getCurrentJointTicksValues()[4]-calibratedInnerTranslate)*encoderTick2mm;
		textFieldActualInnerTranslation.setText(Double.toString(innerTranslation));
		
//		// this would set up the errors
//		double outerTranslationError= Double.parseDouble(textFieldDesiredOuterTranslation.getText())-outerTranslation;		
	}

	public void setCurrentJointTicksValues(int[] currentJointTicksValues) {
		this.currentJointTicksValues = currentJointTicksValues;
	}

	public int[] getCurrentJointTicksValues() {
		return currentJointTicksValues;
	}
	
	
	public double[] getCurrentJointSIValues() {
		return currentJointSIValues;
	}
	
	public void setCurrentJointChannelSIValues(int channel , double currentJointSIValues) {
		this.currentJointSIValues[channel] = currentJointSIValues;
	}
	
	
	public GenericPIDDevice getDevice() {
		return kinematicsGUI.getDevice();
	}

	public synchronized void runDesiredJointUpdate(double seconds) {
		// 1 step: update desired joint
		// reference the jointSpaceVector (integer: encoder ticks)
		setDesiredJointTickValue(0,desiredJointTicksInteger[0]-calibratedOutTranslate);
		setDesiredJointTickValue(1,desiredJointTicksInteger[1]-calibratedMiddleTranslate);
		setDesiredJointTickValue(2,desiredJointTicksInteger[2]);
		setDesiredJointTickValue(3,desiredJointTicksInteger[3]-calibratedMiddleRotate);	
		setDesiredJointTickValue(4,desiredJointTicksInteger[4]-calibratedInnerTranslate);	
		
		System.err.println("Updating motors");
		getDevice().SetPIDSetPoint(0, getDesiredJointTicksValues()[0],seconds);
		getDevice().SetPIDSetPoint(1, getDesiredJointTicksValues()[1],seconds);
		getDevice().SetPIDSetPoint(2, getDesiredJointTicksValues()[2],seconds);
		getDevice().SetPIDSetPoint(3, getDesiredJointTicksValues()[3],seconds);
		getDevice().SetPIDSetPoint(4, getDesiredJointTicksValues()[4],seconds);	
	}
	
	
	public synchronized void runDesiredJointChannelUpdate(int channel, double seconds) {
		int calibrated=0;
		
		 switch (channel) {
		 case 0:  calibrated = calibratedOutTranslate;     //  break;
         case 1:  calibrated = calibratedMiddleTranslate;    //   break;
         case 2:  calibrated = 0;   //   break;
         case 3:  calibrated = calibratedMiddleRotate;       //  break;
         case 4:  calibrated = calibratedInnerTranslate;      //   break;         
         default: calibrated = 0; //break;
     }
		 	 
		setDesiredJointTickValue(channel,desiredJointTicksInteger[channel]+calibrated);		
		System.err.println("Updatging motors");
		getDevice().SetPIDSetPoint(channel, getDesiredJointTicksValues()[channel],seconds);		
	}
	
	
	
	public void setDesiredJointTickValue(int group, int target) {
		getDesiredJointTicksValues()[group] = target;
	}
	
	public int[] getDesiredJointTicksValues() {
		return desiredJointTicksInteger;
	}
	
	
	public void stopPID(){
		for (int i=0;i<5;i++){
			PIDConfiguration conf = new PIDConfiguration();
			conf.setEnabled(false);
			conf.setGroup(i);
			getDevice().ConfigurePIDController(conf);
		}
	}
		
		
	public void resumePID(){
			for (int i=0;i<5;i++){
				PIDConfiguration conf = new PIDConfiguration();
				conf.setEnabled(true);
				conf.setGroup(i);
				getDevice().ConfigurePIDController(conf);
			}
	}
	
	public void setKinematicsGUI(MainGUI kinematicsGUI) {
		this.kinematicsGUI = kinematicsGUI;
	}
}
